// Standard includes 
#include <sys/time.h>

// Local includes
#include "imu/imu.h"

// Constants
#define R_X 0.044/2
#define R_Y 0.025/2
#define R_Z 0.011/2
#define SLEEP_TIME_US 1000
#define SLEEP_TIME_S 0.001
#define RUN_TIME_US 1000000
#define PRINT_TIME_US 100000

// Globals
int timer = 0;
double xPos = 0;
double yPos = 0;
double zPos = 0;

// Function prototypes
double integrate(double, double, double, double);
double getTime(struct timeval);
double absolute(double);

int main(int argc, char **argv) {
	IMU imu = IMU();
	IMUData data;
	double tPrev, t;
	struct timeval tv;

	const char *port = (argc < 2 ? "/dev/ttyACM0" : argv[1]);
	cout << "Attempting to open port: " << port << endl;

	// Make sure the port was opened
	if(imu.openComPort(port)) {
		cout << "Connected. Now scanning for intial data." << endl;
		cout << "dVeloc, dTime, dPos, pos" << endl;

		imu.readData(data);
		double dxVelocPrev = data.dxVeloc;
		double dyVelocPrev = data.dyVeloc;

		gettimeofday(&tv, 0);
		tPrev = getTime(tv);

		while(timer < RUN_TIME_US) {
			imu.readData(data);
			gettimeofday(&tv, 0);
			t = getTime(tv);
			
			double dx = integrate(dxVelocPrev, data.dxVeloc, tPrev, t);
			double dy = integrate(dyVelocPrev, data.dyVeloc, tPrev, t);
			//double dx = data.dxVeloc * (t - tPrev);
			//double dy = data.dyVeloc * (t - tPrev);

			//xPos += (absolute(dx) > 0.01 ? dx : 0.0);		
			//yPos += (absolute(dy) > 0.01 ? dy : 0.0);
			xPos += dx;
			yPos += dy;

			if(timer % PRINT_TIME_US == 0) {
				cout << data.dxVeloc << ", ";
				cout << (t - tPrev) << ", ";
				cout << dx << ", ";
				cout << xPos << endl;

				cout << data.dyVeloc << ", ";
				cout << (t - tPrev) << ", ";
				cout << dy << ", ";
				cout << yPos << endl << endl;
			}

			dxVelocPrev = data.dxVeloc;
			dyVelocPrev = data.dyVeloc;
			tPrev = t;

			usleep(SLEEP_TIME_US);
			timer += SLEEP_TIME_US;
		}

		cout << "Disconnecting." << endl;
		imu.closeComPort();
	}

	return 0;
}

double integrate(double prev, double curr, double lower, double upper) {
	double b = upper - lower, h1, h2;
	
	if(curr > prev) {
		h1 = curr - prev;
		h2 = prev;
	} else {
		h1 = prev - curr;
		h2 = curr;
	}

	return (0.5*b*h1 + b*h2);
}

double getTime(struct timeval time) {
	return (time.tv_sec + time.tv_usec / 1000000.0);
}

double absolute(double val) {
	return (val < 0.0 ? 0.0-val : val);
}



